#include <ros/ros.h> 
// #include <std_msgs/String.h> 
// #include <std_msgs/Empty.h> 
#include <vector>
// #include <math.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Quaternion.h>
// #include <Eigen/Eigen>
// #include <algorithm>
// #include <geometry_msgs/Twist.h>
#include <std_msgs/Float32.h>

using namespace std;

double current_angle[4];
double target_angle[4];
double vel_max = 0.2;
double kp = 10.0;

bool gimbal_target_yaw_cb_flag=false;
ros::Publisher yaw_vel_pub[4];


void gimbal_target_yaw_cb(geometry_msgs::Quaternion msg)
{
    target_angle[0] = msg.x;
    target_angle[1] = msg.y;
    target_angle[2] = msg.z;
    target_angle[3] = msg.w;
    if(!gimbal_target_yaw_cb_flag){
        gimbal_target_yaw_cb_flag = true;
    }
}

void gimbal_yaw_cb0(geometry_msgs::PoseStamped msg)
{
    current_angle[0]=msg.pose.position.x;
}

void gimbal_yaw_cb1(geometry_msgs::PoseStamped msg)
{
    current_angle[1]=msg.pose.position.x;
}

void gimbal_yaw_cb2(geometry_msgs::PoseStamped msg)
{
    current_angle[2]=msg.pose.position.x;
}

void gimbal_yaw_cb3(geometry_msgs::PoseStamped msg)
{
    current_angle[3]=msg.pose.position.x;
}


int main (int argc, char** argv) 
{   
    ros::init(argc, argv, "gimbal_yaw_control"); 

    ros::NodeHandle nh("~");
    nh.getParam("vel_max", vel_max);
    nh.getParam("kp", kp);

    const int ctrl_rate = 30;
    ros::Rate loop_rate(ctrl_rate);

    yaw_vel_pub[0] = nh.advertise<std_msgs::Float32>("/iris_0/gimbal_yaw_vel_cmd", 1);
    yaw_vel_pub[1] = nh.advertise<std_msgs::Float32>("/iris_1/gimbal_yaw_vel_cmd", 1);
    yaw_vel_pub[2] = nh.advertise<std_msgs::Float32>("/iris_2/gimbal_yaw_vel_cmd", 1);
    yaw_vel_pub[3] = nh.advertise<std_msgs::Float32>("/iris_3/gimbal_yaw_vel_cmd", 1);

    ros::Subscriber gimbal_yaw_angle_sub[4];
    gimbal_yaw_angle_sub[0] = nh.subscribe("/iris_0/gimbal_yaw_angle", 1, gimbal_yaw_cb0);
    gimbal_yaw_angle_sub[1] = nh.subscribe("/iris_1/gimbal_yaw_angle", 1, gimbal_yaw_cb1);
    gimbal_yaw_angle_sub[2] = nh.subscribe("/iris_2/gimbal_yaw_angle", 1, gimbal_yaw_cb2);
    gimbal_yaw_angle_sub[3] = nh.subscribe("/iris_3/gimbal_yaw_angle", 1, gimbal_yaw_cb3);

    ros::Subscriber gimbal_yaw_target_angle_sub = nh.subscribe("/swarm_plan/cam_yaw_target_angle", 1, gimbal_target_yaw_cb);

    ROS_INFO("[SWARM_PLAN]: gimbal_yaw_control NODE START");
    while(ros::ok())
    {
        if(gimbal_target_yaw_cb_flag)
        {
            std_msgs::Float32 vel_cmd;
            for (int i = 0; i < 4;i++){
                double delta_angle = target_angle[i] - current_angle[i];
                if(delta_angle>vel_max){
                    delta_angle = vel_max;
                }
                else if (delta_angle < -vel_max){
                    delta_angle = -vel_max;
                }
                vel_cmd.data = delta_angle;
                vel_cmd.data=kp*(delta_angle);
                yaw_vel_pub[i].publish(vel_cmd);
            }
        }

        ros::spinOnce();
        loop_rate.sleep();
    }


	return 0;

}